﻿#include <bits/stdc++.h>
#include "main.h"
#include "robot.h"
#include "astar.h"
#include "init.h"
#include "boat.h"
using namespace std;

Robot robot[robot_num]; // 机器人信息
int astar_times; // 一帧的a*计算次数
int no_task_rbt; // 本帧手上没有任务的机器人数量


// 找到路径最短的泊位
int find_mindis_berth(const Point &src, int pre_bad_berth) {
	int min_path = 1000000;
	int ans = -1;
	for (int i = 0; i < berth_num; i++) {
		int path = all_berth_path[src.x][src.y][i];
		if (path != 0 && pre_bad_berth != i && path < min_path) {
			min_path = path;
			ans = i;
		}
	}
	return ans;
}


// bfs搜索周围货物, mode为0为近距离模式，mode为1为远距离模式
int bfs_search(const Point &src, int depth, int pre_bad_gds, int mode)
{
	// BFS寻找货物
	int ans = -1;
	double min_exp = 1000000;
	queue<Point> q;
	int visit[n][n] = { 0 };
	q.push(src);
	visit[src.x][src.y] = 1;
	int dp = 0;
	int num = 1; // 每一层的点个数
	while (!q.empty()) {
		if (dp >= depth)
			break;
		int temp = 0;
		for (int i = 0; i < num; i++) {
			Point &p = q.front();
			// 判断该点是否满足条件
			int index = gds_map[p.x][p.y];
			if (index != -1 && index != pre_bad_gds && goods_list[index].goods_status != 0 && goods_list[index].survival_time > frame_id + dp + frame_deviation) {
				int dis_to_berth = all_berth_path[goods_list[index].p.x][goods_list[index].p.y][find_mindis_berth(goods_list[index].p, -1)];
				double exp;
				if (mode == 0) {
					exp = (dp + 0.01 * (goods_list[index].survival_time - (frame_id + dp))) * 10.0 / goods_list[index].price;
				} 
				else {
					exp = (dp + 0.08 * (goods_list[index].survival_time - (frame_id + dp))) / goods_list[index].price + 30.0 / goods_list[index].price;
				}
				if (exp == 0)
					return index;
				else if (exp < min_exp) {
					ans = index;
					min_exp = exp;
				}
			}
			// 遍历p周围的4个位置
			for (int j = 0; j < 4; j++) {
			    int x_next = p.x + next_position[j][0];
			    int y_next = p.y + next_position[j][1];
			    if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && ch_map[x_next][y_next] != 'A' && !visit[x_next][y_next]) {
			    	q.push(Point(x_next, y_next));
			    	visit[x_next][y_next] = 1;
			    	++temp;
			    }
			}
			q.pop();
		}
		num = temp;
		++dp; // 深度加一
	}
	
	return ans;
}

// 找商品
static int find_near_goods(int rbt_id, int pre_bad_gds)
{
	const Point &rbt_p = robot[rbt_id].p;
    int num = 300; // 只查前num个商品，让查找时间稳定
    double min_exp = 100000;     // 期望
    int ans = -1;
    int distance; double exp;
    
    // 先用bfs找商品，找不到在去列表中找
    //if (rbt_id % 4 != 0)
	if (maps == 1)
    	ans = bfs_search(rbt_p, 150, pre_bad_gds, 0);
	else
		ans = bfs_search(rbt_p, 180, pre_bad_gds, 0);
    //else 
    //	ans = bfs_search(rbt_p, 200, pre_bad_gds, 1);
    	
    if (ans != -1) {
    	robot[rbt_id].find_gds_failed = 0;
    	return ans;
    } 
    else if (robot[rbt_id].find_gds_failed == 0){
    	robot[rbt_id].find_gds_failed == 1;
    	return -1;
    }

	// bfs找不到再去列表中找
    for (int i = goods_list.size() - 1; i >= 0; i--) 
    {
    	Goods &gds = goods_list[i];
    	if (gds.goods_status == 1) {
			distance = Manhattan(rbt_p, gds.p);
		    if (i != pre_bad_gds && in_map[rbt_p.x][rbt_p.y] == in_map[gds.p.x][gds.p.y] && gds.survival_time - frame_id > distance + frame_deviation) { 
		        exp = distance * 10.0 / goods_list[i].price;
		        if (exp < min_exp) {
		            min_exp = exp;
		            ans = i;
		        }
		    }
        }
        if (--num == 0 || gds.survival_time <= frame_id)
            break;
    }
    return ans;
}

// 找泊位
static int find_near_berth(int rbt_id, int pre_bad_berth)
{
	//return worth_berth[(rbt_id)];
	//return rbt_id;
	const Point &rbt_p = robot[rbt_id].p;
	int ans_min = -1; 
	double exp_min = 1000000;
	
	for (int i = 0; i < robot[rbt_id].reach_berth.size(); i++) {
		int index = robot[rbt_id].reach_berth[i];
		int distance =  all_berth_path[rbt_p.x][rbt_p.y][index];
		double exp = distance;
		int all_goods_num = boat[berth[index].boat_ids.back()].goods_num + berth[index].goods_num;
		// 假如泊位有船，并且还没装满，则增加该泊位的优先级
		if ((berth[index].status == 1 && all_goods_num < boat_capacity) || (berth[index].status == 2 && all_goods_num < 2 * boat_capacity)) {
			if (maps == 2)
				exp -= 15;
			else
				exp -= 20;
			if (maps == 1) {
				if (frame_id >= (frame_num - max_transport_time - 800))
					exp -= 300;
			}
			else {
				if (frame_id >= (frame_num - max_transport_time - 500))
					exp -= 240;
			}
		}

		if (pre_bad_berth != index && berth[index].robot_num < 3 && exp < exp_min) {
			ans_min = index;
			exp_min = exp;
		}
	}
	if (ans_min != -1)
		return ans_min;
	return find_mindis_berth(rbt_p, pre_bad_berth);
}

// 为机器人分配任务
static void robot_compute(int rbt_id)
{
    // 如果携带商品，则计算去泊位的路径
    if (robot[rbt_id].goods == 1) {
        for (int i = 0; i < 2; i++) {
            int berth_id = find_near_berth(rbt_id, robot[rbt_id].pre_bad_berth);
            if (berth_id == -1)
            	return;
            	
	        Point p = berth[berth_id].p;
	        // 放到泊位的随机位置，减少冲突
	        p.x += rand() % 4;
	        p.y += rand() % 4;
	        Astar(robot[rbt_id].p.x, robot[rbt_id].p.y, p.x, p.y, robot[rbt_id].path, 0);
	        if (!robot[rbt_id].path.empty()) {
	            robot[rbt_id].move_target = 2;
	            robot[rbt_id].target = p;
	            robot[rbt_id].target_id = berth_id;
	            robot[rbt_id].path_index = robot[rbt_id].path.size() - 1;
	            ++berth[berth_id].robot_num;
	            robot[rbt_id].pre_bad_berth = -1;
	            return;
	        } 
	        // 假如当前泊位不可达，则记录下来
	        else {
	            robot[rbt_id].pre_bad_berth = berth_id;
	        }
        }
    }
    // 如果未携带商品，则计算去商品的路径
    else {
        for (int i = 0; i < 2; i++) {
            int gds_index = find_near_goods(rbt_id, robot[rbt_id].pre_bad_gp);
            if (gds_index == -1)
                return;
            
            Goods &gds = goods_list[gds_index];
            Astar(robot[rbt_id].p.x, robot[rbt_id].p.y, gds.p.x, gds.p.y, robot[rbt_id].path, 1);
            // 计算出来的路径不为0，且商品的存活时间要大于走路次数
            if (!robot[rbt_id].path.empty() && gds.survival_time - frame_id > robot[rbt_id].path.size() + frame_deviation) {
                robot[rbt_id].move_target = 1;
                robot[rbt_id].target = gds.p;
                robot[rbt_id].goods_take = gds_index;
                robot[rbt_id].path_index = robot[rbt_id].path.size() - 1;
                robot[rbt_id].pre_bad_gp = -1;
                gds.goods_status = 0;
                gds_map[gds.p.x][gds.p.y] = -1;
                return;
            }
            // 若物品不可达或者存活时间小于走路次数，记录下来
            else {
                robot[rbt_id].pre_bad_gp = gds_index;
            }
        }
    }
}

// 比较两个机器人的优先级,rbt1优先级高于rbt2优先级则返回1
static bool compare_priority(int rbt1_id, int rbt2_id)
{
	const Robot &rbt1 = robot[rbt1_id];
	const Robot &rbt2 = robot[rbt2_id];
	
	// 假如机器人避让失败，则提升其优先级
	if (rbt1.avoid_failed == 1) 
		return true;
	else if (rbt2.avoid_failed == 1)
		return false;
		
	// 假如机器人等待次数超过4次，则提高其优先级
	if (rbt1.wait_times >= 4 && rbt1.wait_times >= rbt2.wait_times)
		return true;
	else if (rbt2.wait_times >= 4 && rbt1.wait_times < rbt2.wait_times)
		return false;
		
	// 去拿货物的比去送货物优先级高
	if (rbt1.move_target == 1 && rbt2.move_target != 1)
		return true;
	else if (rbt1.move_target != 1 && rbt2.move_target == 1)
		return false;
	// 两个都要去拿货物，则比较货物过期时间和货物价值
	else if (rbt1.move_target == 1 && rbt2.move_target == 1) {
		double priority1 = (goods_list[rbt1.goods_take].survival_time - (frame_id + rbt1.path_index + frame_deviation)) / goods_list[rbt1.goods_take].price;
        double priority2 = (goods_list[rbt2.goods_take].survival_time - (frame_id + rbt2.path_index + frame_deviation)) / goods_list[rbt2.goods_take].price;
        if (priority1 > 0 && priority1 < priority2)
        	return true;
        else if (priority2 > 0 && priority2 < priority1)
        	return false;
        else 
        	return true;
	}
	// 假如两个机器人都去送货，则目标泊位有船并且货物价格高的优先
	else if (rbt1.move_target == 2 && rbt2.move_target == 2) {
		double priority1 =  (berth[rbt1.target_id].status + 1) * goods_list[rbt1.goods_take].price;
        double priority2 =  (berth[rbt2.target_id].status + 1) * goods_list[rbt2.goods_take].price;
        if (priority1 >= priority2)
        	return true;
        else
        	return false;
	}
	
	return true;
}

void conflict_detect(int rbt_id)
{
	// 机器人没有任务的时候不进行碰撞检测
	if (robot[rbt_id].move_target == 0) {
        return;
    }
    
	robot[rbt_id].avoid_robot.clear();
	robot[rbt_id].avoid_path.clear();
	robot[rbt_id].conflict_point.clear();	
	// 将可能发生冲突的机器人加入冲突列表中
	for (int i = robot[rbt_id].path_index; i >= robot[rbt_id].path_index - min(avoid_range, robot[rbt_id].path_index); i--) {
		for (int j = 0; j < robot_num; j++) {
			if (j == rbt_id)
				continue;
			if (robot[j].move_target == 0)
				continue;
			// 如果已经在冲突机器人数组了，continue;
			if (find(robot[rbt_id].avoid_robot.begin(), robot[rbt_id].avoid_robot.end(), j) != robot[rbt_id].avoid_robot.end())
				continue;
			// 如果自己已在对方避免碰撞数组，跳过
			if (find(robot[j].avoid_robot.begin(), robot[j].avoid_robot.end(), rbt_id) != robot[j].avoid_robot.end())
				continue;
			
			for (int k = robot[j].path_index; k >= robot[j].path_index - min(avoid_range, robot[j].path_index); k--) {
				// 相差点大于1应该就不会相撞了
				if (abs((robot[rbt_id].path_index - i) - (robot[j].path_index - k)) > 1) 
					continue;
				// 如果点重合则会发生冲突
				if (robot[rbt_id].path[i] == robot[j].path[k]) {
					// 优先级低的避让优先级高的机器人
					if (!compare_priority(rbt_id, j)) {
						robot[rbt_id].avoid_robot.push_back(j);
						robot[rbt_id].conflict_point[j] = robot[j].path[k];
					}
				}
			}
				
		}
	}
	
	if (!robot[rbt_id].avoid_robot.empty()) {
		// 让冲突机器人到冲突点的路径变为障碍，并且不可选择冲突机器人后10个路径点作为避障点
		int temp_map[n][n] = { 0 };
		for (int i = 0; i < robot[rbt_id].avoid_robot.size(); i++) {
			int av_rbt_id = robot[rbt_id].avoid_robot[i];
			Robot &av_rbt = robot[av_rbt_id];
			int j;
			for (j = av_rbt.path_index; j >= av_rbt.path_index - min(avoid_range, av_rbt.path_index); j--) {
				Point &p = av_rbt.path[j];
				if (robot[rbt_id].p == p) 
					break;
				temp_map[p.x][p.y] = -1;
				if (robot[rbt_id].conflict_point[av_rbt_id] == p) {
					--j;
					break;
				}
			}
			for ( ; j >= max(av_rbt.path_index - 7, 0); j--) {
				Point &p = av_rbt.path[j];
				if (temp_map[p.x][p.y] == 0)
					temp_map[p.x][p.y] = 1;
			}
		}
		
		// BFS寻找合适的避障点
		Point target;
		queue<Point> q;
		int visit[n][n] = {0};
		int flag = 0; // 是否找到避障点
		q.push(robot[rbt_id].p);
		visit[robot[rbt_id].p.x][robot[rbt_id].p.y] = 1;
		while (!q.empty()) {
			Point p = q.front();
			// 判断该点是否满足条件，若找到避让点，则跳出循环
			if (temp_map[p.x][p.y] != -1 && temp_map[p.x][p.y] != 1 && ch_map[p.x][p.y] != '#' && ch_map[p.x][p.y] != '*' && ch_map[p.x][p.y] != 'A') {
				flag = 1;
				target = p;
				break;
			}
			q.pop();
			// 遍历p周围的4个位置
		    for (int i = 0; i < 4; i++) {
		        int x_next = p.x + next_position[i][0];
		        int y_next = p.y + next_position[i][1];
		        if (x_next >= 0 && x_next < n && y_next >= 0 && y_next < n && ch_map[x_next][y_next] != '#' && ch_map[x_next][y_next] != '*' && ch_map[x_next][y_next] != 'A' && !visit[x_next][y_next] && temp_map[x_next][y_next] != -1) {
		        	q.push(Point(x_next, y_next));
		        	visit[x_next][y_next] = 1;
		        }
			}
		}
		
		// 成功找到避让点则通过a*计算路径
		if (flag) {
			astar_conflict(robot[rbt_id].p.x, robot[rbt_id].p.y, target.x, target.y, robot[rbt_id].avoid_path, 1);
			robot[rbt_id].avoid_failed = 0;
		} 
		else {
			robot[rbt_id].avoid_failed = 1;
			robot[rbt_id].avoid_robot.clear();
			//fprintf(stderr, "%d avoid point not found!\n", frame_id);
		    //fflush(stdout);
		}
		
		// 将避障路径合并到主路径中
		if (robot[rbt_id].avoid_path.size() > 1) {
			//fprintf(stderr, "%d\n", robot[rbt_id].avoid_path.size());
		    //fflush(stdout);
			for (int i = robot[rbt_id].avoid_path.size() - 2; i >= 0; i--) {
				robot[rbt_id].path.push_back(robot[rbt_id].avoid_path[i]);
			}
			for (int i = 1; i < robot[rbt_id].avoid_path.size(); i++) {
				robot[rbt_id].path.push_back(robot[rbt_id].avoid_path[i]);
			}
			robot[rbt_id].path_index = robot[rbt_id].path.size() - 1;
		}
    }
} 

// 机器人执行任务
void robot_task(int rbt_id)
{
    // 如果机器人没有可达泊位，则直接废弃该机器人
    if (robot[rbt_id].reach_berth_status == 2)
        return;

    // 机器人状态为恢复状态，则不执行任务
    if (robot[rbt_id].status == 0) {
        // 如果机器人上一帧为正常，则说明机器人上一帧的移动发生了碰撞，需要恢复一些信息
        if (robot[rbt_id].pre_status == 1) {
            robot[rbt_id].rand_frame = rand() % 3; // 生成随机暂停帧，减少冲突
            if (robot[rbt_id].move_target == 2)
                --berth[robot[rbt_id].target_id].robot_num;
	        else if (robot[rbt_id].move_target == 1 && goods_list[robot[rbt_id].goods_take].survival_time > frame_id + frame_deviation) {
                goods_list[robot[rbt_id].goods_take].goods_status = 1;
                gds_map[goods_list[robot[rbt_id].goods_take].p.x][goods_list[robot[rbt_id].goods_take].p.y] = robot[rbt_id].goods_take;
            }
            robot[rbt_id].move_target = 0;
            robot[rbt_id].is_move = 0;
            robot[rbt_id].is_get = 0;
            robot[rbt_id].is_pull = 0;
            robot[rbt_id].pre_status = 0;
            
            ++crack_times;
        }
        return;
    }

    // 假如机器人上一帧移动成功了，则更新移动信息
    if (robot[rbt_id].is_move) {
        robot[rbt_id].is_move = 0;
        // 移动成功
        if (robot[rbt_id].path[robot[rbt_id].path_index - 1] == robot[rbt_id].p) {
		    --robot[rbt_id].path_index;
		    robot[rbt_id].path.pop_back();
		    p_map[robot[rbt_id].mp] = -1;
		    ch_map[robot[rbt_id].mp.x][robot[rbt_id].mp.y] = '.';
		    ch_map[robot[rbt_id].p.x][robot[rbt_id].p.y] = 'A';
		    robot[rbt_id].wait_times = 0;
        } 
        else {
        	p_map[robot[rbt_id].path[robot[rbt_id].path_index - 1]] = -1;
        	fprintf(stderr, "%d move failed!\n", frame_id);
            fflush(stdout);
        }
    }

    // 假如机器人上一帧get成功了，则更新物品信息
    if (robot[rbt_id].is_get) {
        robot[rbt_id].is_get = 0;
        if (robot[rbt_id].goods == 1) {
        	get_goods_sum++;
        	get_goods_prices += goods_list[robot[rbt_id].goods_take].price;
        }
        // get成功或者货物已超时
        if (robot[rbt_id].goods == 1 || goods_list[robot[rbt_id].goods_take].survival_time < frame_id) {
        	robot[rbt_id].move_target = 0;
        }
    }

    // 假如机器人上一帧pull成功了，则更新泊位信息
    if (robot[rbt_id].is_pull) {
        robot[rbt_id].is_pull = 0;
        // pull成功
        if (robot[rbt_id].goods == 0) {
		    int berth_id = robot[rbt_id].target_id;
		    int goods_price = goods_list[robot[rbt_id].goods_take].price;
		    --berth[berth_id].robot_num;
		    //if (berth[berth_id].goods_index == 0)
		    berth[berth_id].goods_price_list.push_front(goods_price);
		    //else 
		    //	berth[berth_id].goods_price_list.back() += goods_price;
		    berth[berth_id].goods_price += goods_price;
		    ++berth[berth_id].goods_num;
		    berth[berth_id].wait_time = 0;
		    //berth[berth_id].goods_index = (berth[berth_id].goods_index + 1) % berth[berth_id].loading_speed;
		    robot[rbt_id].move_target = 0;
        }
    }
    
    // 假如机器人到达不了商品，则取消该任务
    if (robot[rbt_id].move_target == 1 && (frame_id + robot[rbt_id].path_index + frame_deviation) >= goods_list[robot[rbt_id].goods_take].survival_time) {
    	if (goods_list[robot[rbt_id].goods_take].survival_time > frame_id + frame_deviation) {
        	goods_list[robot[rbt_id].goods_take].goods_status = 1;
        	gds_map[goods_list[robot[rbt_id].goods_take].p.x][goods_list[robot[rbt_id].goods_take].p.y] = robot[rbt_id].goods_take;
        }
    	robot[rbt_id].move_target = 0;
    }

    // 机器人没有任务时，为其分配任务
    if (robot[rbt_id].move_target == 0) {
        robot_compute(rbt_id);
    }
    
    // 碰撞检测
    conflict_detect(rbt_id);

    // 机器人手上有任务的时候，发出指令
    if (robot[rbt_id].move_target != 0) {
        /** 移动前动作 **/
        if (robot[rbt_id].target == robot[rbt_id].p) {
            // 如果目标是商品，且机器人位置和目标商品重合，则获取商品
            if (robot[rbt_id].move_target == 1  && robot[rbt_id].goods == 0) {
                get(rbt_id);
                robot[rbt_id].is_get = 1;
            }
            // 如果目标是泊位，且机器人位置和泊位重合，则放下商品
            else if (robot[rbt_id].move_target == 2  && robot[rbt_id].goods == 1) {
                pull(rbt_id);
                robot[rbt_id].is_pull = 1;
            }
            else {
            	robot[rbt_id].move_target == 0;
            }
        }

        /** 移动 **/
        // 如果随机暂停帧rand_frame不为0则让机器人不动
        if (robot[rbt_id].rand_frame > 0) {
            --robot[rbt_id].rand_frame;
        }
        // 如果机器人需要移动，则发出移动指令
        else if (robot[rbt_id].path_index > 0) {
        	int index = robot[rbt_id].path_index;
            const Point &p = robot[rbt_id].path[index - 1];
            // 假如机器人的下一个位置会和其他机器人碰撞
            auto it = p_map.find(p);
            if (it != p_map.end() && it->second != -1) {
            	++crack_times;
            	//int crack_rbt_id = it->second;
            	++robot[rbt_id].wait_times;
            	// 
            	if (robot[rbt_id].wait_times >= 2) {
            		robot[rbt_id].rand_frame = rand() % 3; // 生成随机暂停帧，减少冲突
            		if (robot[rbt_id].move_target == 2)
            			--berth[robot[rbt_id].target_id].robot_num;
            		else if (robot[rbt_id].move_target == 1 && goods_list[robot[rbt_id].goods_take].survival_time > frame_id + frame_deviation) {
            			goods_list[robot[rbt_id].goods_take].goods_status = 1;
            			gds_map[goods_list[robot[rbt_id].goods_take].p.x][goods_list[robot[rbt_id].goods_take].p.y] = robot[rbt_id].goods_take;
            		}
            		robot[rbt_id].move_target = 0;
            	}
            	//if (compare_priority(rbt_id, crack_rbt_id)) {
            		//robot[rbt_id].rand_frame = 1; // 生成暂停帧，减少冲突
            		//++robot[rbt_id].wait_times;
            		
            		//if (robot[crack_rbt_id].move_target == 2)
            		//	--berth[robot[crack_rbt_id].target_id].robot_num;
            		//else if (robot[crack_rbt_id].move_target == 1 && robot[crack_rbt_id].goods_take.survival_time > frame_id + frame_deviation)
            		//	goods_list.push_front(robot[crack_rbt_id].goods_take);
            		//robot[crack_rbt_id].move_target = 0;
            	//} 
            	//else {
            		//robot[crack_rbt_id].rand_frame = 1; // 生成暂停帧，减少冲突
            		//++robot[crack_rbt_id].wait_times;
            		//if (robot[rbt_id].move_target == 2)
            		//	--berth[robot[rbt_id].target_id].robot_num;
            		//else if (robot[rbt_id].move_target == 1 && robot[rbt_id].goods_take.survival_time > frame_id + frame_deviation)
            		//	goods_list.push_front(robot[rbt_id].goods_take);
            		//robot[rbt_id].move_target = 0;
            	//}
            }
            // 假如不会碰撞，则发出移动指令
            else {
                int dir = getDirect(robot[rbt_id].p, p); // 获取移动方向
                if (dir == 8) {
                	fprintf(stderr, "%d stopped!\n", frame_id);
                    fflush(stdout);
                }
                else if (dir != -1) {
                    move(rbt_id, dir); // 移动
                    robot[rbt_id].mp = robot[rbt_id].p; // 记录移动前位置
                    robot[rbt_id].is_move = 1;
                    p_map[p] = rbt_id;
                }
                else {
                    fprintf(stderr, "%d dir is -1!\n", frame_id);
                    fflush(stdout);
                }
            }
        }

        /** 移动后动作，注意是移动后才有移动后动作 **/
        if (robot[rbt_id].is_move) {
        	int index = robot[rbt_id].path_index;
            Point p = robot[rbt_id].path[index - 1];
            // 机器人位置和目标重合
            if (robot[rbt_id].target == p) {
                // 如果目标是商品，则获取商品
                if (robot[rbt_id].move_target == 1 && robot[rbt_id].goods == 0) {
                    get(rbt_id);
                    robot[rbt_id].is_get = 1;
                }
                // 如果目标是泊位，则放下商品
                else if (robot[rbt_id].move_target == 2 && robot[rbt_id].goods == 1) {
                    pull(rbt_id);
                    robot[rbt_id].is_pull = 1;
                }
                else {
                	robot[rbt_id].move_target == 0;
                }
            }
        }
    }
    
    robot[rbt_id].pre_status = robot[rbt_id].status;
}
